#pragma once
#include "spacepartitioner.h"
#include "Math\tribox.h"
#include <vector>
#include <fstream>
#include "GLaddon/OpenGLFunc.h"
#pragma  warning(disable : 4018)

static bool bDrawUnitBox = false;

template<class FT>
class COctreeNode{
public:
	COctreeNode()
	{
		child[0] = NULL;
		child[1] = NULL;
		child[2] = NULL;
		child[3] = NULL;
		child[4] = NULL;
		child[5] = NULL;
		child[6] = NULL;
		child[7] = NULL;
		t = 0;
		depth = 0;
		c[0] = 0.0f;
		c[1] = 0.0f;
		c[2] = 0.0f;
		size = 0.0f;
	}
	~COctreeNode()
	{
	}

	COctreeNode *child[8];
	unsigned char t;
	int depth;
	FT c[3];
	FT size;
	std::vector<int> m_ctn_Fid;
	std::vector<int> m_int_Fid;	
};

template <class Polyhedron, class Kernel>
class COctreePartitioner :
	public CSpacePartitioner<Polyhedron, Kernel>
{
	typedef	typename Kernel::FT			FT;
	typedef std::vector<int> Vec_int;
	typedef Vec_int::const_iterator Vec_int_iterator;
	typedef typename Polyhedron::Facet_iterator Facet_iterator;
	typedef	typename Kernel::Point_3	Point;
	typedef typename Polyhedron::Halfedge_around_facet_circulator HF_circulator;

public:
	COctreePartitioner(void) : CSpacePartitioner()
	{
		m_nDepth = m_nMaxDepth = 0;
		root = NULL;
		m_fRes = m_fMaxRes = 1.0f;
	}
	COctreePartitioner(Polyhedron *pMesh) : CSpacePartitioner(pMesh)
	{
		m_nDepth = m_nMaxDepth = 0;
		m_fRes = m_fMaxRes = 1.0f;
		root = NULL;
	}
	COctreePartitioner(Polyhedron *pMesh, int nMaxDepth) : CSpacePartitioner(pMesh)
	{
		m_nMaxDepth = nMaxDepth;
		m_fMaxRes = 1.0f / pow(2.0, m_nMaxDepth);
		root = NULL;
		m_fRes = 1.0f;
		m_nDepth = 0;
	}
	~COctreePartitioner(void)
	{
		deletePartition(root);
	}

	void PartitionMesh(void)
	{
		if (m_pMesh == NULL) {
			AfxMessageBox("Mesh cannot be NULL!");
			return;
		}
		if (!m_pMesh->is_unitized()) {
			m_pMesh->unitize();
		}
		root = new COctreeNode<FT>;
		root->c[0] = 0.0f;
		root->c[1] = 0.0f;
		root->c[2] = 0.0f;
		root->depth = 1;
		root->size = m_pMesh->get_unitizing_scale_factor(); 
		root->t = 2;	// partially filled by Mesh
		Vec_int ft;		// facet table
		int i=0;
		for(Facet_iterator pFacet=m_pMesh->facets_begin(); pFacet!=m_pMesh->facets_end(); pFacet++, i++)
		{
			ft.push_back(i);
		}
		//////////////////////////////////////////////////////////////////////////
		partitionWithDepth(root, 1, ft);
		m_nDepth = m_nMaxDepth;
		m_fRes = m_fMaxRes;

		serialize_NonEmpty_LeafNodes();
	}

	void DrawSpatialPartition(void)
	{
		FT dBBoxHSize = 0.5 * root->size;
		glPushAttrib(GL_ENABLE_BIT|GL_CURRENT_BIT);
		glDisable(GL_LIGHTING);
		glColor3f(0.0f, 0.0f, 0.0f);
		// Draw unit box
		if(bDrawUnitBox){
			glBegin(GL_LINE_LOOP);
			glVertex3F(dBBoxHSize, dBBoxHSize, dBBoxHSize);
			glVertex3F(-dBBoxHSize, dBBoxHSize, dBBoxHSize);
			glVertex3F(-dBBoxHSize, -dBBoxHSize, dBBoxHSize);
			glVertex3F(dBBoxHSize, -dBBoxHSize, dBBoxHSize);
			glEnd();
			glBegin(GL_LINE_LOOP);
			glVertex3F(dBBoxHSize, -dBBoxHSize, -dBBoxHSize);
			glVertex3F(dBBoxHSize, dBBoxHSize, -dBBoxHSize);
			glVertex3F(-dBBoxHSize, dBBoxHSize, -dBBoxHSize);
			glVertex3F(-dBBoxHSize, -dBBoxHSize, -dBBoxHSize);
			glEnd();
			glBegin(GL_LINES);
			glVertex3F(dBBoxHSize, dBBoxHSize, dBBoxHSize);
			glVertex3F(dBBoxHSize, dBBoxHSize, -dBBoxHSize);
			glVertex3F(-dBBoxHSize, dBBoxHSize, dBBoxHSize);
			glVertex3F(-dBBoxHSize, dBBoxHSize, -dBBoxHSize);
			glVertex3F(-dBBoxHSize, -dBBoxHSize, dBBoxHSize);
			glVertex3F(-dBBoxHSize, -dBBoxHSize, -dBBoxHSize);
			glVertex3F(dBBoxHSize, -dBBoxHSize, dBBoxHSize);
			glVertex3F(dBBoxHSize, -dBBoxHSize, -dBBoxHSize);
			glEnd();
		}
		glBegin(GL_LINES);
		// Draw partition
// 		if (root != NULL) {
// 			//drawNode(root);
// 			drawLeafNodesByIter(root);
// 		}
		draw_NonEmpty_LeafNodes();
		glEnd();
		glPopAttrib();
	}

	bool StoreProfile(const CString &sFileName)
	{
		if (root == NULL) {
			AfxMessageBox("StoreOSPProfile: empty octree!");
			return false;
		}
		std::ofstream stream(sFileName);
		if (!stream)
		{
			CString s;
			s = "StoreOSPProfile: cannot open file " + sFileName;
			AfxMessageBox(s);
			return false;
		}
		stream << m_pMesh->get_unitizing_scale_factor() << std::endl;
		stream << m_nMaxDepth << std::endl;
		writeNode(stream, root);
		return true;
	}

	bool LoadProfile(const CString &sFileName)
	{
		std::ifstream stream(sFileName);
		if (!stream)
		{
			CString s;
			s = "LoadOSPProfile: cannot open file " + sFileName;
			AfxMessageBox(s);
			return false;
		}
		FT dSF = 1.0;
		stream >> dSF;
		stream >> m_nMaxDepth;
		if (root != NULL) {
			deletePartition(root);
		}
		readNode(stream, root);
		if (!m_pMesh->is_unitized()
			|| dSF!=m_pMesh->get_unitizing_scale_factor()) 
		{
			m_pMesh->unitize(dSF);
		}
		return true;
	}
	/*Glossary:
	NE: non-empty
	NEL: non-empty leaf nodes
	NEL_ctn_F: facets contained by non-empty leaf nodes
	NEL_int_F: facets intersected with non-empty leaf nodes
	*/
	std::vector< COctreeNode<FT>* > m_NE_Leaves; //non empty leaf nodes

	void deletePartition(COctreeNode<FT> *node)
	{
		if (node != NULL) {
			return;
		}
		for (int i=0; i<m_nDepth; i++) {
			deletePartition(node->child[i]);
		}
		delete node;
		root = NULL;
	}

	void partitionWithDepth(COctreeNode<FT> *node, int depth, const Vec_int &pft)
	{
		if ((++depth) > m_nMaxDepth) {
			return;
		}
		node->t = 3;	// subdivided
		FT size = node->size * 0.5f;
		FT hsize = size * 0.5f;
		FT *c = &(node->c[0]);
		
		node->child[0] = new COctreeNode<FT>;
		node->child[0]->c[0] = c[0]-hsize;
		node->child[0]->c[1] = c[1]-hsize;
		node->child[0]->c[2] = c[2]-hsize;
		node->child[0]->size = size;
		node->child[0]->depth = depth;
		Vec_int ft1;
		if ((node->child[0]->t=getFillByMesh(node->child[0]->c, hsize, pft, ft1)) == 2) {
			partitionWithDepth(node->child[0], depth, ft1);
			if(depth==m_nMaxDepth)	node->child[0]->m_int_Fid = ft1;
		}		

		node->child[1] = new COctreeNode<FT>;
		node->child[1]->c[0] = c[0]+hsize;
		node->child[1]->c[1] = c[1]-hsize;
		node->child[1]->c[2] = c[2]-hsize;
		node->child[1]->size = size;
		node->child[1]->depth = depth;
		Vec_int ft2;
		if ((node->child[1]->t=getFillByMesh(node->child[1]->c, hsize, pft, ft2)) == 2) {
			partitionWithDepth(node->child[1], depth, ft2);
			if(depth==m_nMaxDepth)	node->child[1]->m_int_Fid = ft2;
		}

		node->child[2] = new COctreeNode<FT>;
		node->child[2]->c[0] = c[0]+hsize;
		node->child[2]->c[1] = c[1]+hsize;
		node->child[2]->c[2] = c[2]-hsize;
		node->child[2]->size = size;
		node->child[2]->depth = depth;
		Vec_int ft3;
		if ((node->child[2]->t=getFillByMesh(node->child[2]->c, hsize, pft, ft3)) == 2) {
			partitionWithDepth(node->child[2], depth, ft3);
			if(depth==m_nMaxDepth)	node->child[2]->m_int_Fid = ft3;
		}

		node->child[3] = new COctreeNode<FT>;
		node->child[3]->c[0] = c[0]-hsize;
		node->child[3]->c[1] = c[1]+hsize;
		node->child[3]->c[2] = c[2]-hsize;
		node->child[3]->size = size;
		node->child[3]->depth = depth;
		Vec_int ft4;
		if ((node->child[3]->t=getFillByMesh(node->child[3]->c, hsize, pft, ft4)) == 2) {
			partitionWithDepth(node->child[3], depth, ft4);
			if(depth==m_nMaxDepth)	node->child[3]->m_int_Fid = ft4;
		}

		node->child[4] = new COctreeNode<FT>;
		node->child[4]->c[0] = c[0]-hsize;
		node->child[4]->c[1] = c[1]+hsize;
		node->child[4]->c[2] = c[2]+hsize;
		node->child[4]->size = size;
		node->child[4]->depth = depth;
		Vec_int ft5;
		if ((node->child[4]->t=getFillByMesh(node->child[4]->c, hsize, pft, ft5)) == 2) {
			partitionWithDepth(node->child[4], depth, ft5);
			if(depth==m_nMaxDepth)	node->child[4]->m_int_Fid = ft5;
		}

		node->child[5] = new COctreeNode<FT>;
		node->child[5]->c[0] = c[0]+hsize;
		node->child[5]->c[1] = c[1]+hsize;
		node->child[5]->c[2] = c[2]+hsize;
		node->child[5]->size = size;
		node->child[5]->depth = depth;
		Vec_int ft6;
		if ((node->child[5]->t=getFillByMesh(node->child[5]->c, hsize, pft, ft6)) == 2) {
			partitionWithDepth(node->child[5], depth, ft6);
			if(depth==m_nMaxDepth)	node->child[5]->m_int_Fid = ft6;
		}

		node->child[6] = new COctreeNode<FT>;
		node->child[6]->c[0] = c[0]+hsize;
		node->child[6]->c[1] = c[1]-hsize;
		node->child[6]->c[2] = c[2]+hsize;
		node->child[6]->size = size;
		node->child[6]->depth = depth;
		Vec_int ft7;
		if ((node->child[6]->t=getFillByMesh(node->child[6]->c, hsize, pft, ft7)) == 2) {
			partitionWithDepth(node->child[6], depth, ft7);
			if(depth==m_nMaxDepth)	node->child[6]->m_int_Fid = ft7;
		}

		node->child[7] = new COctreeNode<FT>;
		node->child[7]->c[0] = c[0]-hsize;
		node->child[7]->c[1] = c[1]-hsize;
		node->child[7]->c[2] = c[2]+hsize;
		node->child[7]->size = size;
		node->child[7]->depth = depth;
		Vec_int ft8;
		if ((node->child[7]->t=getFillByMesh(node->child[7]->c, hsize, pft, ft8)) == 2) {
			partitionWithDepth(node->child[7], depth, ft8);
			if(depth==m_nMaxDepth)	node->child[7]->m_int_Fid = ft8;
		}
	}

	unsigned char getFillByMesh(FT *c, FT r, const Vec_int &pft, Vec_int &ft)
	{
		// if intersect with mesh
		FT hsize[3];
		hsize[0] = hsize[1] = hsize[2] = r;
		Vec_int_iterator f;
		BOOL bPartial = FALSE;
		HF_circulator pHalfedge;
		Point v1, v2, v3;
		FT va[3], vb[3], vc[3];
		ft.clear();
		for (f=pft.begin(); f!=pft.end(); f++) {
			pHalfedge = (m_pMesh->index_to_facet_map[(*f)])->facet_begin();
			v1 = pHalfedge->vertex()->point();
			v2 = pHalfedge->next()->vertex()->point();
			v3 = pHalfedge->next()->next()->vertex()->point();
			va[0] = v1.x();
			va[1] = v1.y();
			va[2] = v1.z();
			vb[0] = v2.x();
			vb[1] = v2.y();
			vb[2] = v2.z();
			vc[0] = v3.x();
			vc[1] = v3.y();
			vc[2] = v3.z();
			if (triBoxOverlap(c, hsize, va, vb, vc)) 
			{
				ft.push_back(*f);
				bPartial = TRUE;
			}
		}
		if (bPartial) {
			return 2;
		} else {
			if (isInsideModel(c)) {	// if covered by mesh
				return 1;
			} else {
				return 0;
			}
		}
	}

	void drawNode(COctreeNode<FT> *node)
	{
		ASSERT(node != NULL);

		if ( node->t !=3 ) {
			return;
		}
		FT hsize = node->size * 0.5f;		
		
		/////////////////////////3 middle axis//////////////////////////////////
		glVertex3F(node->c[0]-hsize, node->c[1], node->c[2]);
		glVertex3F(node->c[0]+hsize, node->c[1], node->c[2]);
		glVertex3F(node->c[0], node->c[1]-hsize, node->c[2]);
		glVertex3F(node->c[0], node->c[1]+hsize, node->c[2]);
		glVertex3F(node->c[0], node->c[1], node->c[2]-hsize);
		glVertex3F(node->c[0], node->c[1], node->c[2]+hsize);
		/////////////////////////6 crosses////////////////////////////////////
		// Front
		glVertex3F(node->c[0]-hsize, node->c[1], node->c[2]+hsize);
		glVertex3F(node->c[0]+hsize, node->c[1], node->c[2]+hsize);
		glVertex3F(node->c[0], node->c[1]-hsize, node->c[2]+hsize);
		glVertex3F(node->c[0], node->c[1]+hsize, node->c[2]+hsize);
		// Back
		glVertex3F(node->c[0]-hsize, node->c[1], node->c[2]-hsize);
		glVertex3F(node->c[0]+hsize, node->c[1], node->c[2]-hsize);
		glVertex3F(node->c[0], node->c[1]-hsize, node->c[2]-hsize);
		glVertex3F(node->c[0], node->c[1]+hsize, node->c[2]-hsize);
		// Left
		glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]);
		glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]);
		glVertex3F(node->c[0]-hsize, node->c[1], node->c[2]-hsize);
		glVertex3F(node->c[0]-hsize, node->c[1], node->c[2]+hsize);
		// Right
		glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]);
		glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]);
		glVertex3F(node->c[0]+hsize, node->c[1], node->c[2]-hsize);
		glVertex3F(node->c[0]+hsize, node->c[1], node->c[2]+hsize);
		// Bottom
		glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]);
		glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]);
		glVertex3F(node->c[0], node->c[1]-hsize, node->c[2]-hsize);
		glVertex3F(node->c[0], node->c[1]-hsize, node->c[2]+hsize);
		// Top
		glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]);
		glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]);
		glVertex3F(node->c[0], node->c[1]+hsize, node->c[2]-hsize);
		glVertex3F(node->c[0], node->c[1]+hsize, node->c[2]+hsize);
		// draw children
		for (int i=0; i<8; i++) {
			if (node->child[i] != NULL) {
				drawNode(node->child[i]);
			}
		}
	}
	
	void serialize_NonEmpty_LeafNodes(){
		m_NE_Leaves.clear();
		search_NonEmpty_Leaf_DFS(root);
		std::cout<<"Number of leaf nodes: "<<m_NE_Leaves.size()<<"\n";
		/*check how many triangles are overlapped with this leaf node cube*/
		std::cout<<"check how many triangles are overlapped with this leaf node cube\n";
		int nNEL = m_NE_Leaves.size();
		for (int i=0; i<nNEL; ++i){
			std::cout<<"cube "<<i<<" has "<<m_NE_Leaves[i]->m_int_Fid.size()<<"int_Tris: ";
			std::copy(m_NE_Leaves[i]->m_int_Fid.begin(), m_NE_Leaves[i]->m_int_Fid.end(),  std::ostream_iterator<int>(std::cout,", "));
			std::cout<<"\n";
		}
		std::cout<<"\n";
	}
	void search_NonEmpty_Leaf_DFS(COctreeNode<FT> *node){
		if(node==NULL) return;
		if( node->depth!=m_nMaxDepth){
			for (int i=0; i<8; ++i)
				search_NonEmpty_Leaf_DFS(node->child[i]);
			return;
		}
		else{			
			if(node->t!= 2) return;
			m_NE_Leaves.push_back(node);
		}
	}

	void draw_NonEmpty_LeafNodes_DFS(COctreeNode<FT> *node){
		
		if(node==NULL) return;
		
		if( node->depth!=m_nMaxDepth){
			for (int i=0; i<8; ++i)
				draw_NonEmpty_LeafNodes_DFS(node->child[i]);
			return;
		}
		else{
			FT hsize = node->size * 0.5f;		
			if(node->t!= 2) return;
			//
			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);
			
			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);
			
			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);

			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);
			
			//
			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);
			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);

			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);

			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);

			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);
		
			//
			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);
			
			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);
			
			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);
			
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);
			glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);
		}
	}


	void draw_NonEmpty_LeafNodes(){

		for (unsigned i=0; i<m_NE_Leaves.size(); ++i)
		{
			FT hsize = m_NE_Leaves[i]->size * 0.5f;		
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);

			//
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);

			//
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]+hsize, m_NE_Leaves[i]->c[1]-hsize, m_NE_Leaves[i]->c[2]-hsize);

			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]+hsize);
			glVertex3F(m_NE_Leaves[i]->c[0]-hsize, m_NE_Leaves[i]->c[1]+hsize, m_NE_Leaves[i]->c[2]-hsize);			
		}			
	}

	void writeNode(std::ofstream &ofs, COctreeNode<FT> *node)
	{
		ASSERT(node!=NULL);
		ofs << node->c[0] << " " 
			<< node->c[1] << " " 
			<< node->c[2] << " " 
			<< node->size << " " 
			<< node->t << std::endl;
		if (node->t == 3) {
			for (int i=0; i<8; i++) {
				if (node->child[i] != NULL) {
					writeNode(ofs, node->child[i]);
				}
			}
		}
	}

	void readNode(std::ifstream &ifs, COctreeNode<FT> *&node)
	{
		node = new COctreeNode<FT>;
		ifs >> node->c[0] >> node->c[1] >> node->c[2] >> node->size >> node->t;
		if (node->t == 3) {
			for (int i=0; i<8; i++) {
				readNode(ifs, node->child[i]);
			}
		}
	}

	int pick_cube_by_ray(Vector3 &sp, Vector3 &d, float epsilon,bool bMuiltipick=false,bool bUnpick = false)
	{
		int pi = -1;
		float min_t = FLT_MAX;
		float min_dist = epsilon, dist;
		float t = 0.0f;
		Vector3 vert, vec;

		d.normalize();
		
		int nNEL = m_NE_Leaves.size();
		for (int i=0; i<nNEL; ++i)
		{
			vert.set(m_NE_Leaves[i]->c[0], m_NE_Leaves[i]->c[1], m_NE_Leaves[i]->c[2]);
			t = d.dot(vert - sp);
			if (t <= 0 || t >= min_t) {
				continue;
			}
			vec = vert - (sp + d * t);
			dist = vec.sqauremagnitude();
			if (dist < min_dist) {
				pi = i;	// vertex index
				min_dist = dist;
				min_t = t;
			}
		}
		return pi;		
	}

	bool pick_cube_by_idx(int cid)
	{
		int nNEL = m_NE_Leaves.size();
		if( cid<0||cid>nNEL-1 )
			return false;
		return true;		
	}
	
	int pick_cube_by_column(Vector3 &sp, Vector3 &dx, Vector3 &dy, Vector3 &dz)
	{
		float t1 = 0.0f, t2 = 0.0f, t3 = 0.0f;

		float width = dx.magnitude();
		float height = dy.magnitude();
		dx.normalize();
		dy.normalize();
		dz.normalize();

		Vector3 vec, vert, norml;
		int first_Cube_idx = -1;
		int nNEL = m_NE_Leaves.size();
		for (int i=0; i<nNEL; ++i)
		{
			vert.set(m_NE_Leaves[i]->c[0], m_NE_Leaves[i]->c[1], m_NE_Leaves[i]->c[2]);
			vec = vert - sp;
			if (dx.dot(vec)<=0 || dy.dot(vec)<=0 || dz.dot(vec)<=0)	continue;
			if (width < fabs(dx.dot(vec)))							continue;
			if (height < fabs(dy.dot(vec)))							continue;
									
			first_Cube_idx = i;	break;
		}		
		return first_Cube_idx;
	}

	void gl_draw_picked_cube(int cid)
	{
		if ( cid<0||cid>=m_NE_Leaves.size() ) {
			return;
		}
		COctreeNode<FT> *node = m_NE_Leaves[cid];
		FT hsize = node->size * 0.5f;				

		::glPushAttrib(GL_ALL_ATTRIB_BITS);
		::glDisable(GL_TEXTURE_2D);
		::glEnable(GL_LIGHTING);
		::glDisable(GL_DEPTH_TEST);
		::glDepthMask(GL_FALSE);
		::glEnable(GL_BLEND);
		::glDisable(GL_CULL_FACE);
		::glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
		::glEnable(GL_COLOR_MATERIAL);		
		::glColor4f(0.1f, 0.2f, 0.85f, 0.5f);
		::glPolygonMode(GL_FRONT, GL_FILL);
		::glPolygonMode(GL_BACK, GL_LINE);
				
		glBegin(GL_QUADS);
			// Front					
			glNormal3f(0,0,1);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);//0
			glNormal3f(0,0,1);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);//1
			glNormal3f(0,0,1);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);//2
			glNormal3f(0,0,1);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);//3

			// Right			
			glNormal3f(1,0,0);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);//0
			glNormal3f(1,0,0);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);//4
			glNormal3f(1,0,0);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);//5
			glNormal3f(1,0,0);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);//1
			// Left
			glNormal3f(-1,0,0);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);//3	
			glNormal3f(-1,0,0);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);//2					
			glNormal3f(-1,0,0);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);//6
			glNormal3f(-1,0,0);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);//7			

			// Back						
			glNormal3f(0,0,-1);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);//7
			glNormal3f(0,0,-1);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);//6
			glNormal3f(0,0,-1);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);//5
			glNormal3f(0,0,-1);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);//4
			
			// Top				
			glNormal3f(0,1,0);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]+hsize);//0
			glNormal3f(0,1,0);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]+hsize);//3
			glNormal3f(0,1,0);glVertex3F(node->c[0]-hsize, node->c[1]+hsize, node->c[2]-hsize);//7
			glNormal3f(0,1,0);glVertex3F(node->c[0]+hsize, node->c[1]+hsize, node->c[2]-hsize);//4

			// Bottom			
			glNormal3f(0,-1,0);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]+hsize);//1
			glNormal3f(0,-1,0);glVertex3F(node->c[0]+hsize, node->c[1]-hsize, node->c[2]-hsize);//5
			glNormal3f(0,-1,0);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]-hsize);//6
			glNormal3f(0,-1,0);glVertex3F(node->c[0]-hsize, node->c[1]-hsize, node->c[2]+hsize);//2			
			
			
		glEnd();
		::glPopAttrib();
	}

private:
	int m_nMaxDepth;
	FT m_fMaxRes;
	int m_nDepth;
	FT m_fRes;
	COctreeNode<FT> *root;
};